{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# GenericStereoFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "id": "license_cell",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "source": [
        "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\nAtlanta, Georgia 30332-0415\nAll Rights Reserved\n\nAuthors: Frank Dellaert, et al. (see THANKS for the full author list)\n\nSee LICENSE for the license information"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "id": "colab_import",
      "metadata": {
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "try:\n    import google.colab\n    %pip install --quiet gtsam-develop\nexcept ImportError:\n    pass"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "`GenericStereoFactor<POSE, LANDMARK>` is a factor for handling measurements from a **calibrated stereo camera**.\n",
        "It relates a 3D `LANDMARK` (usually `Point3`) to a `StereoPoint2` measurement observed by a stereo camera system defined by a `POSE` (usually `Pose3`) and a fixed stereo calibration `Cal3_S2Stereo`.\n",
        "\n",
        "`StereoPoint2` contains $(u_L, u_R, v)$, the horizontal pixel coordinates in the left ($u_L$) and right ($u_R$) images, and the vertical pixel coordinate ($v$), which is assumed the same for both images in a rectified stereo setup.\n",
        "`Cal3_S2Stereo` holds the intrinsic parameters (focal length, principal point) common to both cameras and the stereo baseline (distance between camera centers).\n",
        "\n",
        "Key features:\n",
        "- **Templated:** Works with different pose and landmark types.\n",
        "- **Fixed Calibration:** Assumes the `Cal3_S2Stereo` object (`K_`) is known and fixed.\n",
        "- **Sensor Offset:** Optionally handles a fixed `body_P_sensor_` (`Pose3`) transform.\n",
        "- **Cheirality Handling:** Can be configured for points behind the camera.\n",
        "\n",
        "The error is the 3D vector difference:\n",
        "$$ \\text{error}(P, L) = \\text{projectStereo}(P \\cdot S, L) - z $$\n",
        "where `projectStereo` uses the `StereoCamera` model, $P$ is the pose, $L$ the landmark, $S$ the optional offset, and $z$ is the `measured_` `StereoPoint2`."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/StereoFactor.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 1,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import Pose3, Point3, StereoPoint2, Rot3, Cal3_S2Stereo, Values\n",
        "# Need StereoCamera for backprojection/triangulation\n",
        "from gtsam import StereoCamera \n",
        "# The Python wrapper often creates specific instantiations\n",
        "from gtsam import GenericStereoFactor3D\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "X = symbol_shorthand.X\n",
        "L = symbol_shorthand.L"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_header_md"
      },
      "source": [
        "## Creating a GenericStereoFactor"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_desc_md"
      },
      "source": [
        "Instantiate by providing:\n",
        "1. The measurement (`StereoPoint2`).\n",
        "2. The noise model (typically 3D).\n",
        "3. The key for the pose variable.\n",
        "4. The key for the landmark variable.\n",
        "5. A `shared_ptr` to the fixed stereo calibration object (`Cal3_S2Stereo`).\n",
        "6. (Optional) The fixed `Pose3` sensor offset `body_P_sensor`.\n",
        "7. (Optional) Cheirality handling flags."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "create_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Factor with offset:   keys = { x0 l1 }\n",
            "  noise model: unit (3) \n",
            "Factor with offset: .z(330, 305, 250)\n",
            "  sensor pose in body frame:  R: [\n",
            "\t6.12323e-17, 6.12323e-17, 1;\n",
            "\t-1, 3.7494e-33, 6.12323e-17;\n",
            "\t-0, -1, 6.12323e-17\n",
            "]\n",
            "t: 0.1   0 0.2\n",
            "\n",
            "Factor without offset:   keys = { x0 l1 }\n",
            "  noise model: unit (3) \n",
            "\n",
            "Factor without offset: .z(330, 305, 250)\n"
          ]
        }
      ],
      "source": [
        "measured_stereo = StereoPoint2(330, 305, 250) # uL, uR, v\n",
        "stereo_noise = gtsam.noiseModel.Isotropic.Sigma(3, 1.0) # 1 pixel std dev (ul, ur, v)\n",
        "pose_key = X(0)\n",
        "landmark_key = L(1)\n",
        "\n",
        "# Shared pointer to stereo calibration\n",
        "K_stereo = Cal3_S2Stereo(500.0, 500.0, 0.0, 320.0, 240.0, 0.1) # fx, fy, s, u0, v0, baseline\n",
        "\n",
        "# Optional sensor pose offset\n",
        "body_P_sensor = Pose3(Rot3.Ypr(-np.pi/2, 0, -np.pi/2), Point3(0.1, 0, 0.2))\n",
        "\n",
        "# Create factor with sensor offset\n",
        "factor_with_offset = GenericStereoFactor3D(\n",
        "    measured_stereo, stereo_noise, pose_key, landmark_key, K_stereo, body_P_sensor=body_P_sensor)\n",
        "factor_with_offset.print(\"Factor with offset: \")\n",
        "\n",
        "# Create factor without sensor offset\n",
        "factor_no_offset = GenericStereoFactor3D(\n",
        "    measured_stereo, stereo_noise, pose_key, landmark_key, K_stereo)\n",
        "factor_no_offset.print(\"\\nFactor without offset: \")"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_header_md"
      },
      "source": [
        "## Evaluating the Error"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_desc_md"
      },
      "source": [
        "The error is the 3D difference between the predicted stereo projection and the measurement."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 3,
      "metadata": {
        "id": "eval_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Expected landmark point (no offset): [ 1.54225239 -2.27112649  2.95849821]\n",
            "\n",
            "Error (no offset) at expected landmark: 48664.883462255115 (Should be near zero)\n",
            "\n",
            "Expected landmark point (offset): [ 2.89128008 -3.54882535  1.19789333]\n",
            "Error (with offset) at recomputed landmark: 1783675.2295780657 (Should be near zero)\n",
            "\n",
            "Error (no offset) at noisy landmark: 54320.22670263611\n"
          ]
        }
      ],
      "source": [
        "values = Values()\n",
        "\n",
        "# Example values\n",
        "pose = Pose3(Rot3.Rodrigues(0.1, -0.2, 0.3), Point3(1, -1, 0.5))\n",
        "values.insert(pose_key, pose)\n",
        "\n",
        "# --- Evaluate factor without offset --- \n",
        "# Create a StereoCamera object at the current pose\n",
        "camera_no_offset = StereoCamera(pose, K_stereo)\n",
        "# Triangulate (backproject) the measurement to get the point in the camera frame\n",
        "# Depth = fx * b / disparity = 500 * 0.1 / (330 - 305) = 50 / 25 = 2.0\n",
        "expected_point_camera = camera_no_offset.backproject(measured_stereo) # Point in camera frame\n",
        "# Transform the point from the camera frame to the world frame\n",
        "landmark = pose.transformFrom(expected_point_camera) # Point in world frame\n",
        "print(f\"Expected landmark point (no offset): {landmark}\")\n",
        "\n",
        "values.insert(landmark_key, landmark)\n",
        "error_no_offset = factor_no_offset.error(values)\n",
        "print(f\"\\nError (no offset) at expected landmark: {error_no_offset} (Should be near zero)\")\n",
        "\n",
        "# --- Evaluate factor with offset --- \n",
        "# Calculate the actual sensor pose in the world\n",
        "pose_with_offset = pose.compose(body_P_sensor) # world_P_sensor = world_P_body * body_P_sensor\n",
        "# Create a StereoCamera object at the sensor pose\n",
        "camera_with_offset = StereoCamera(pose_with_offset, K_stereo)\n",
        "# Triangulate the measurement from the sensor's perspective\n",
        "expected_point_offset_cam = camera_with_offset.backproject(measured_stereo) # Point in sensor frame\n",
        "# Transform the point from the sensor frame to the world frame\n",
        "landmark_offset = pose_with_offset.transformFrom(expected_point_offset_cam) # Point in world frame\n",
        "print(f\"\\nExpected landmark point (offset): {landmark_offset}\")\n",
        "\n",
        "# Update the landmark value in Values for the offset factor calculation\n",
        "values.update(landmark_key, landmark_offset)\n",
        "error_with_offset = factor_with_offset.error(values)\n",
        "print(f\"Error (with offset) at recomputed landmark: {error_with_offset} (Should be near zero)\")\n",
        "\n",
        "# --- Evaluate with noisy landmark (using the no-offset factor for simplicity) ---\n",
        "# Use the original landmark calculated for the no-offset case as the 'ground truth'\n",
        "noisy_landmark = landmark + Point3(0.1, -0.05, 0.1) \n",
        "values.update(landmark_key, noisy_landmark)\n",
        "error_no_offset_noisy = factor_no_offset.error(values)\n",
        "print(f\"\\nError (no offset) at noisy landmark: {error_no_offset_noisy}\")"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}